﻿stdafx.cpp : 僅包含標準 Include 檔的原始程式檔
Standard Form.pch 會成為先行編譯標頭檔
stdafx.obj 會包含先行編譯型別資訊
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include "stdafx.h"


void Vision::initial()
{
	this->VI = new videoInput();

	//open the camera
	this->VI->setupDevice(this->camID ,this->camHeight ,this->camWidth);

	//malloc image matrix
	this->camFrame = cvCreateImage(cvSize(this->camWidth ,this->camHeight) ,IPL_DEPTH_8U ,3);
	this->camFrame_HSV = cvCreateImage(cvSize(this->camWidth ,this->camHeight) ,IPL_DEPTH_8U ,3);

	//set the pixels to zero
	this->isPixelSearched = cv::Mat::zeros(this->camHeight ,this->camWidth ,CV_8UC1);
}

void Vision::run( unsigned short objIndex )
{ 
	switch ( info->eventIndex )
	{

		case FIRA_EVENT::TEST_MODE :	this->findObject( objIndex, 1000 );	break;
		case FIRA_EVENT::SPRINT:		this->findAuxiliary_sprint();		break;
		case FIRA_EVENT::WEIGHT_LIFTING:									break;
		case FIRA_EVENT::CLIMBING:		this->findStairs();					break;
		case FIRA_EVENT::OBSTACLERUN:	this->findObstacles();				break;
			// 			switch( objIndex )
			// 			{
			// 			case OBJECT::OBSTACLERUN::B_OB:
			// 				this->findObject(OBJECT::OBSTACLERUN::B_OB);
			// 				break;
			// 
			// 			case OBJECT::OBSTACLERUN::Y_OB:
			// 				this->findObject(OBJECT::OBSTACLERUN::Y_OB);
			// 				break;
			// 
			// 			case OBJECT::OBSTACLERUN::R_OB:
			// 				this->findObject(OBJECT::OBSTACLERUN::R_OB);
			// 				break;
			// 
			// 			default:
			// 				break;
			// 			}
			//break;
		case FIRA_EVENT::PK:
			switch( objIndex )
			{
				case OBJECT::PK::PK_BALL:	this->findUSball();			break;
				case OBJECT::PK::PK_OB: this->findPKObstacle();			break;
				default:												break;
			}
			break;
	
		case FIRA_EVENT::MARATHON:
			switch( objIndex )
			{
				case OBJECT::MARATHON::LINE:	this->findMarathonLine();	break;
				case OBJECT::MARATHON::ARROW:	this->findMarathonGap();	break;
				default:													break;
			}
			break;

		case FIRA_EVENT::BASKETBALL:
			switch( objIndex )
			{
				case OBJECT::BASKETBALL::B_BALL:	this->findBasketball();		break;
				case OBJECT::BASKETBALL::B_BASKET:	this->findBasketHoop();		break;
				default:														break;
			}
			break;

		case FIRA_EVENT::LC:
			switch( objIndex )
			{
				case  OBJECT::LC::LEVELONE:		this->findLevelOne();			break;
				case  OBJECT::LC::LEVELTWO:		this->findLevelTwo();			break;
				case  OBJECT::LC::LEVELTHREE:	this->findLevelThree();			break;
			}
		default:
			printf( "\nPlease check the dip.\nThere is no object in vision\n" );
			break;
	}
}
/*---------------- MARATHON---------------------*/
bool Vision::findMarathonLine()
{
	//判斷線
	if ( this->findObject(OBJECT::MARATHON::LINE,500) ) //有看到line
	{
		std::vector<ObjectRegion>::iterator marathonLine = info->objectRegion[OBJECT::AUXILIARY::COLOR1].begin();
		
		this->ED_DAimg = &IplImage( marathonLine->markedImage ); //加入侵蝕和膨脹
 		cvErode( this->ED_DAimg, this->ED_DAimg, NULL, this->errodenumbers );
		cvDilate(this-> ED_DAimg, this->ED_DAimg, NULL, this->dilatenumbers );
		
		//line selected initial
		this->ED_leftboundary = 640;
		this->ED_rightboundary = 0;
		this->ED_topboundary = 480;
		this->ED_bottomboundary = 0;

		for ( int j=0 ; j<info->vision.camHeight ; j++)
		{
			for ( int i=0 ; i<info->vision.camWidth ; i++)
			{
				if ( marathonLine->markedImage.at<unsigned char>(j,i) ) //有碰到白線
				{
					this->x_value = i; //水平 <---->
					this->y_value = j; //上下
					if ( this->x_value < this->ED_leftboundary )
						this->ED_leftboundary = x_value;
					if ( this->x_value > this->ED_rightboundary )
						this->ED_rightboundary = x_value;
					if ( this->y_value < this->ED_topboundary )
						this->ED_topboundary = y_value;
					if ( this->y_value > this->ED_bottomboundary )
						this->ED_bottomboundary = y_value;
				}
			}
		}
		cvRectangle(this->ED_DAimg ,cvPoint(this->ED_leftboundary , this->ED_topboundary) ,cvPoint(this->ED_rightboundary , this->ED_bottomboundary) ,CV_RGB(255,255,255) ,1);
		//cvShowImage( "Line" ,ED_DAimg );
		cv::imshow( "Line Map" ,marathonLine->markedImage );
		cv::waitKey( 100 );

		printf( "\nVision DEA lineang = %0.f\n",marathonLine->panAngle ); //vision這邊經過侵蝕膨脹後的角度
		info->strategy.adult.Marathon.lineEDAang = marathonLine->panAngle;

		/* 將找到的線放入地圖中 */
		int i=0,j=0;
		double lineAngle = 0.0;
		/* 尋找線的四個頂點 計算線的角度 */
		//找下面
		j = this->ED_bottomboundary;
		for (i = this->ED_leftboundary ; i < this->ED_rightboundary ; i++ )
		{
			if(marathonLine->markedImage.at<unsigned char>(j,i))
				break;
		}
		cv::Point2d leftBottom(i,j);
		for (i = this->ED_rightboundary ; i > this->ED_leftboundary ; i-- )
		{
			if(marathonLine->markedImage.at<unsigned char>(j,i))
				break;
		}
		cv::Point2d rightBottom(i,j);

		// 找上面 
		j = this->ED_topboundary;
		for (i = this->ED_leftboundary ; i < this->ED_rightboundary ; i++ )
		{
			if(marathonLine->markedImage.at<unsigned char>(j,i))
				break;
		}
		cv::Point2d leftTop(i,j);

		for (i = this->ED_rightboundary;i > this->ED_leftboundary;i--)
		{
			if(marathonLine->markedImage.at<unsigned char>(j,i))
				break;
		}
		cv::Point2d rightTop(i,j);
		double linebottomMainX;
		double linebottompanAng;

		lineAngle = -atan((rightTop.x-rightBottom.x)/(rightTop.y-rightBottom.y))*180/CV_PI;
		lineAngle += (-atan((leftTop.x-leftBottom.x)/(leftTop.y-leftBottom.y))*180/CV_PI);
		lineAngle /= 2;
		info->strategy.adult.Marathon.lineAngle = lineAngle;
		//printf( "\n lineAng = %0.f\n",lineAngle );

		/* 顯示 */
		// 		cv::imshow( "Line Map" ,marathonLine->markedImage );
		// 		cv::waitKey( 100 );

// 		unsigned int recordBoundary[480];
// 		unsigned bool recordEnable[480];
// 
// 		for ( int k = 0; k < 480; k++ )
// 		{
// 			recordEnable[k] = false;
// 			recordBoundary[k] = 0;
// 		}
// 
// 		int dt = 15;
// 		for ( j = marathonLine->topBoundary; j < marathonLine->bottomBoundary; j = j+dt )
// 		{
// 			for (i=marathonLine->leftBoundary;i<marathonLine->rightBoundary;i++)
// 			{
// 				if(marathonLine->markedImage.at<unsigned char>(j,i))
// 				{
// 					recordEnable[j] = true;
// 					recordBoundary[j] = i;
// 					/*printf("record high = %d, width = %d\n\n\n",j ,i );*/
// 					break;
// 				}
// 
// 			}
// 		}

		// 		info->strategy.adult.marathon.trendAngle = 
		// 			-atan( ((double)recordBoundary[marathonLine->topBoundary+dt]-(double)recordBoundary[marathonLine->topBoundary])/
		// 				   (double)dt)*180/CV_PI;		


		//decide the main point
		marathonLine->mainPoint.x = ( marathonLine->rightBoundary + marathonLine->leftBoundary ) / 2;
		marathonLine->mainPoint.y = ( marathonLine->topBoundary + marathonLine->bottomBoundary ) / 2;
		linebottomMainX = ( rightBottom.x + leftBottom.x )/2;
		linebottompanAng = atan((rightBottom.x-320)/(480-rightBottom.y) )*180/CV_PI;

		//printf("\n底邊中心點角度 = %0.f\n",linebottompanAng );

// 		if ( linebottompanAng <= 5 || linebottompanAng > 90 ) //防止太近的時候角度過大
// 			linebottompanAng = 0;

		this->xmainpoint = marathonLine->mainPoint.x;
		this->ymainpoint = marathonLine->mainPoint.y;
		info->strategy.adult.Marathon.linebottomPanAng= linebottompanAng;
		info->strategy.adult.Marathon.linebottom = marathonLine->bottomBoundary;

		/*printf("debug vision 11\n");*/
		marathonLine->transformImage2RobotCoordinate();
		marathonLine->calcDisAndAngle();
		return true;
	}

	else  //都沒有
		info->objectRegion[OBJECT::MARATHON::LINE].clear();
}
bool Vision::findMarathonGap()
{
	return true;
}
/*-------------- OBSTACLESRUN-------------------*/
bool Vision::findObstacles()
{
	static int k = 0 ; //用來記錄找到藍色OB的個數
	if ( this->findObject( OBJECT::OBSTACLERUN::OBSTACLE_Y ,1000) )
	{
		std::vector<ObjectRegion>::iterator Y = info->objectRegion[OBJECT::OBSTACLERUN::OBSTACLE_Y].begin();
		info->strategy.adult.obstacleRun.obY_left = Y->leftBoundary;
		info->strategy.adult.obstacleRun.obY_right = Y->rightBoundary;
		info->strategy.adult.obstacleRun.obY_top = Y->topBoundary;
		info->strategy.adult.obstacleRun.obY_bottom = Y->bottomBoundary;

		if ( Y->mainPoint.x > 320 )
			info->strategy.adult.obstacleRun.isYellowLeft = true;
		else
			info->strategy.adult.obstacleRun.isYellowLeft = false;

		//判斷是方形或是邊界線
		double Y_height = abs( Y->bottomBoundary - Y->topBoundary );
		double Y_width = abs( Y->rightBoundary - Y->leftBoundary );
		double Ymax,Ymin;
		
		if ( Y_height >= Y_width )
		{
			Ymax = Y_height;
			Ymin = Y_width;
		}
		else
		{
			Ymax = Y_width;
			Ymin = Y_height ;
		}
		info->strategy.adult.obstacleRun.Y_ratial = Ymax/Ymin;
		
		if ( Y->pixelNumbers < 6000 )
		{
			//黃色太少 先靠近
			info->strategy.adult.obstacleRun.isYellowOB = false;
			info->strategy.adult.obstacleRun.isYellowLine = false;
			//printf( "\n黃色太少\n");
		}
		else
		{
			if ( info->strategy.adult.obstacleRun.Y_ratial > info->strategy.adult.obstacleRun.Y_ratialTH )
			{
				// 長寬比大於threshold 為邊界線
				if ( Y->bottomBoundary < 80 )//表示還很遠(但pixcels夠) 所以還無法真正判斷是否為線或障礙物
				{
					info->strategy.adult.obstacleRun.isYellowOB = false;
					info->strategy.adult.obstacleRun.isYellowLine = false;
					//printf("\n還很遠 所以還無法真正判斷是否為線或障礙物\n");
				}
				else
				{
					info->strategy.adult.obstacleRun.isYellowOB = false;
					info->strategy.adult.obstacleRun.isYellowLine = true;
					//printf("\n避障邊界線\n");
				}	
			}
			else
			{
				//長寬比很接近，比較pixcels數
				info->strategy.adult.obstacleRun.Y_range = Y_height*Y_width;
				double YrangeRate = Y->pixelNumbers / info->strategy.adult.obstacleRun.Y_range;

				if ( YrangeRate >= info->strategy.adult.obstacleRun.Y_rangeTH )
				{
					//為黃色障礙物
					info->strategy.adult.obstacleRun.isYellowOB = true;
					info->strategy.adult.obstacleRun.isYellowLine = false;
					//printf("\n避障黃色障礙物\n");
				}
				else
				{
					//為邊界線
					info->strategy.adult.obstacleRun.isYellowOB = false;
					info->strategy.adult.obstacleRun.isYellowLine = true;
					//printf("\n邊界線\n");
				}
			}
		}

	}
	else
		info->objectRegion[OBJECT::OBSTACLERUN::OBSTACLE_Y].clear();

	if ( this->findObject( OBJECT::OBSTACLERUN::OBSTACLE_R,1000 ) )
	{
		std::vector<ObjectRegion>::iterator R = info->objectRegion[OBJECT::OBSTACLERUN::OBSTACLE_R].begin();
		info->strategy.adult.obstacleRun.obR_left = R->leftBoundary;
		info->strategy.adult.obstacleRun.obR_right = R->rightBoundary;
		info->strategy.adult.obstacleRun.obR_top = R->topBoundary;
		info->strategy.adult.obstacleRun.obR_bottom = R->bottomBoundary;
	}
	else
		info->objectRegion[OBJECT::OBSTACLERUN::OBSTACLE_R].clear();

	RegionOfInterest ROII;
	ROII.setRegion( 0 , camWidth-1 , 0 , camHeight-1);

	if ( this->findObject( OBJECT::OBSTACLERUN::OBSTACLE_B , 2000 , ROII , true ) ) //see blue obstacles
	{
		std::vector<ObjectRegion>::iterator it = info->objectRegion[OBJECT::OBSTACLERUN::OBSTACLE_B].begin();
		info->strategy.adult.obstacleRun.ob_b_pixcels = it->pixelNumbers;
	}
	else 
	{
		info->objectRegion[OBJECT::OBSTACLERUN::OBSTACLE_B].clear();
		info->strategy.adult.obstacleRun.ob_b_pixcels = 0;
	}
	// 	info->objectRegion[OBJECT::OBSTACLERUN::OBSTACLE_B].clear();
	// 	info->objectRegion[OBJECT::OBSTACLERUN::OBSTACLE_Y].clear();
	// 	info->objectRegion[OBJECT::OBSTACLERUN::OBSTACLE_R].clear();
	return true;
}

/*----------------- SPRINT ---------------------*/
bool Vision::findAuxiliary()
{
	//如果上次有找到，先找LOCAL
		if ( !info->objectRegion[OBJECT::AUXILIARY::COLOR1].empty() ){
			std::vector<ObjectRegion>::iterator mainMark = info->objectRegion[OBJECT::AUXILIARY::COLOR1].begin();
	
			RegionOfInterest ROI;
			ROI.setRegion( mainMark->leftBoundary ,mainMark->rightBoundary ,mainMark->topBoundary ,mainMark->bottomBoundary );
	
			if( !this->findObject(OBJECT::AUXILIARY::COLOR1,500,ROI,false) )
				if( !this->findObject(OBJECT::AUXILIARY::COLOR1,500,true) )		//沒找到再找GLOBAL
					return false;//若還是沒找到就RETURN
		}else{
			if( !this->findObject(OBJECT::AUXILIARY::COLOR1,500,true) )//上次根本就沒找到，直接找GLOBAL
				return false;//若還是沒找到就RETURN
		}
	
		//有可能有找到，檢查中間輔助物
		std::vector<ObjectRegion>::iterator mainMark = info->objectRegion[OBJECT::AUXILIARY::COLOR1].begin();
		for( ;mainMark != info->objectRegion[OBJECT::AUXILIARY::COLOR1].end() ;++mainMark )
		{
			RegionOfInterest ROI;
			ROI.setRegion( (*mainMark).leftBoundary  ,(*mainMark).rightBoundary ,(*mainMark).topBoundary ,(*mainMark).bottomBoundary ,0.6 );
	
			if ( this->findObject( OBJECT::AUXILIARY::COLOR2 ,100 ,ROI ,false ) )
			{//找到左邊中間輔助物
	
				int markWidth = mainMark->rightBoundary - mainMark->leftBoundary;
				ROI.setRegion( mainMark->rightBoundary ,mainMark->rightBoundary+markWidth ,mainMark->topBoundary ,mainMark->bottomBoundary );
	
				if ( this->findObject( OBJECT::AUXILIARY::COLOR3 ,500 ,ROI ,false ) )
				{//找到右邊輔助物
	
					std::vector<ObjectRegion>::iterator secondMark = info->objectRegion[OBJECT::AUXILIARY::COLOR3].begin();
					ROI.setRegion( secondMark->leftBoundary ,secondMark->rightBoundary ,secondMark->topBoundary ,secondMark->bottomBoundary ,0.6 );
	
					if ( this->findObject( OBJECT::AUXILIARY::COLOR4 ,100 ,ROI ,false ) )
					{//找到右邊中間輔助物
						//clear all the other "main_mark"s, except the main one
						ObjectRegion tempRegion = (*mainMark);
						info->objectRegion[OBJECT::AUXILIARY::COLOR1].clear();
						info->objectRegion[OBJECT::AUXILIARY::COLOR1].push_back(tempRegion);
	
						return true;
					}
				}
			}
		}
	
		//最後很可以還是找不到
		info->objectRegion[OBJECT::AUXILIARY::COLOR1].clear();
		info->objectRegion[OBJECT::AUXILIARY::COLOR2].clear();
		info->objectRegion[OBJECT::AUXILIARY::COLOR3].clear();
		info->objectRegion[OBJECT::AUXILIARY::COLOR4].clear();
		return false;

	RegionOfInterest ROI;
	ROI.setRegion(0,640,0,480);
	if (this->findObject(OBJECT::AUXILIARY::COLOR1,500,ROI,false))
	{
		std::vector<ObjectRegion>::iterator goal = info->objectRegion[OBJECT::AUXILIARY::COLOR1].begin();

		goal->transformImage2RobotCoordinate();
		goal->calcDisAndAngle();

		return true;

	}
	return false;

}
bool Vision::findAuxiliary_sprint() 
{
	//先找左邊的MARK--------------------------------------------------------------------

	//如果上次有找到，那就用上次local的值，否則就全部重找
	if ( !info->objectRegion[OBJECT::AUXILIARY::COLOR2].empty() )
	{
		//將我們要的資料存到vector，然後去使用
		std::vector<ObjectRegion>::iterator mainMark = info->objectRegion[OBJECT::AUXILIARY::COLOR2].begin();

		//使用上一次找到的LOCAL位置，去搜尋
		RegionOfInterest ROI;
		ROI.setRegion( mainMark->leftBoundary ,mainMark->rightBoundary ,mainMark->topBoundary ,mainMark->bottomBoundary );

		if( !this->findObject( OBJECT::AUXILIARY::COLOR2,200,ROI,false) )
			if( !this->findObject(OBJECT::AUXILIARY::COLOR2,200,true) )		//沒找到再找GLOBAL，可能有多個符合的候選人
				return false;//若還是沒找到就RETURN
	}
	else {
		if( !this->findObject(OBJECT::AUXILIARY::COLOR2,200,true) )//上次根本就沒找到，直接找GLOBAL，可能有多個符合的候選人
			return false;//若還是沒找到就RETURN
	}

	//因為找到的候選人有很多，所以要搭配中間的輔助物來確定是否為我們要的MARK
	std::vector<ObjectRegion>::iterator mainMark = info->objectRegion[OBJECT::AUXILIARY::COLOR2].begin();

	for( ;mainMark != info->objectRegion[OBJECT::AUXILIARY::COLOR2].end() ; ++mainMark )
	{
			//開始找左邊中間的輔助物，邊界設定
			RegionOfInterest ROI;
			double zoomRatio = 0.0;
			int markWidth = mainMark->rightBoundary - mainMark->leftBoundary;
			int markHeight = mainMark->bottomBoundary - mainMark->topBoundary;

			int newLeftBoundary = (int)((*mainMark).leftBoundary - (1-zoomRatio)*markWidth);
			int newRightBoundary = (int)((*mainMark).rightBoundary + (1-zoomRatio)*markWidth);
			int newTopBoundary = (int)((*mainMark).topBoundary - (1-zoomRatio)*markHeight);
			int newBottomBoundary = (int)((*mainMark).bottomBoundary + (1-zoomRatio)*markHeight);

			//開始找左邊的輔助物
			ROI.setRegion( newLeftBoundary , newRightBoundary , newTopBoundary ,newBottomBoundary );

			if( this->findObject( OBJECT::AUXILIARY::COLOR1, 200, ROI, false) )
			{
				//開始找右邊的輔助物，邊界設定
				newLeftBoundary = mainMark->rightBoundary - markWidth; ;
				newRightBoundary = mainMark->rightBoundary + markWidth;
				newTopBoundary = mainMark->topBoundary - markHeight;
				newBottomBoundary = mainMark->bottomBoundary + markHeight;
				ROI.setRegion( newLeftBoundary , newRightBoundary , newTopBoundary ,newBottomBoundary );

				//開始找右邊的輔助物
				if ( this->findObject( OBJECT::AUXILIARY::COLOR3 ,200 ,ROI ,false ) )//找到了，計算資訊
				{
					//due to the height of MARK is not in the ground.
					mainMark->setBottomCenter2MainPoint(); //設 Object 下面中心點是正中央
					if ( info->robot == ROBOT::ADULT )
						mainMark->robotCoordinate.y = 130;
					mainMark->transformImage2RobotCoordinate();
					mainMark->calcDisAndAngle();

					//左右邊的mark
					std::vector<ObjectRegion>::iterator leftMark = info->objectRegion[OBJECT::AUXILIARY::COLOR1].begin();
					leftMark->setLeftCenter2MainPoint();
					leftMark->transformImage2RobotCoordinate();
					leftMark->calcDisAndAngle();

					std::vector<ObjectRegion>::iterator rightMark = info->objectRegion[OBJECT::AUXILIARY::COLOR3].begin();
					rightMark->setRightCenter2MainPoint();
					rightMark->transformImage2RobotCoordinate();
					rightMark->calcDisAndAngle();

					//clear all the other possible Center, except the main one
					ObjectRegion tempRegion = (*mainMark);
					info->objectRegion[OBJECT::AUXILIARY::COLOR2].clear();
					info->objectRegion[OBJECT::AUXILIARY::COLOR2].push_back(tempRegion);

					return true;
				}
			}
	}

	//最後很可能都找不到，就把全部清空，下次再來
	info->objectRegion[OBJECT::AUXILIARY::COLOR1].clear();
	info->objectRegion[OBJECT::AUXILIARY::COLOR2].clear();
	info->objectRegion[OBJECT::AUXILIARY::COLOR3].clear();

	return false;
}

/*------------------ PK ----------------------*/
bool Vision::findUSball()
{
// 	if ( !info->objectRegion[OBJECT::PK::PK_BALL].empty() )
// 	{
// 		std::vector<ObjectRegion>::iterator ball = info->objectRegion[OBJECT::PK::PK_BALL].begin();
// 		RegionOfInterest ROI;
// 		ROI.setRegion(ball->leftBoundary,ball->rightBoundary,ball->topBoundary,ball->bottomBoundary);
// 		info->strategy.kalmanFilter(&ball->robotCoordinate);
// 		ball->transformImage2RobotCoordinate();
// 		ball->calcDisAndAngle();
// 		return true;
// 	}
// 	return false;
	//bool isFound = findObject( OBJECT::PK::PK_BALL ,100 );

	if ( findObject( OBJECT::PK::PK_BALL ,1000 ) )
	{
		std::vector<ObjectRegion>::iterator ball = info->objectRegion[OBJECT::PK::PK_BALL].begin();
		RegionOfInterest ROI;
		ROI.setRegion(ball->leftBoundary,ball->rightBoundary,ball->topBoundary,ball->bottomBoundary);
		info->strategy.kalmanFilter(&ball->robotCoordinate);
		ball->transformImage2RobotCoordinate();
		ball->calcDisAndAngle();
		return true;
	}

	return false;

}
bool Vision::findPKObstacle()
{
	if ( findObject( OBJECT::PK::PK_OB ,1000 ) )
	{
		std::vector<ObjectRegion>::iterator ob = info->objectRegion[OBJECT::PK::PK_OB].begin();
		RegionOfInterest ROI;
		ROI.setRegion(ob->leftBoundary,ob->rightBoundary,ob->topBoundary,ob->bottomBoundary);
		info->strategy.kalmanFilter(&ob->robotCoordinate);
		ob->transformImage2RobotCoordinate();
		ob->calcDisAndAngle();
		return true;
	}
	return false;
}

/* --------- Basketball Event -------------- */
bool Vision::findBasketball()
{
	if( this->findObject(OBJECT::BASKETBALL::B_BALL,800) )
	{
		std::vector<ObjectRegion>::iterator ball = info->objectRegion[OBJECT::BASKETBALL::B_BALL].begin();
		//due to the height of ball is not in the ground.
		/*ball->robotCoordinate.y = info->strategy.adult.basketball.ballHeight;*/

		ball->transformImage2RobotCoordinate();
		ball->calcDisAndAngle();

		if ( /*ball->dis > 2000 ||*/ ball->dis < 0 )
		{
			 info->objectRegion[OBJECT::BASKETBALL::B_BALL].clear();
			 return false;
		}

		return true;
	}

	return false;
}
bool Vision::findBasketHoop()
{
	if (!info->objectRegion[OBJECT::BASKETBALL::B_BASKET].empty())
	{//如果上次有找到，先找LOCAL
		std::vector<ObjectRegion>::iterator basket = info->objectRegion[OBJECT::BASKETBALL::B_BASKET].begin();
		RegionOfInterest ROI;
		ROI.setRegion(basket->leftBoundary,basket->rightBoundary,basket->topBoundary,basket->bottomBoundary);
		if(!this->findObject(OBJECT::BASKETBALL::B_BASKET,100,ROI,false))
			if(!this->findObject(OBJECT::BASKETBALL::B_BASKET,100,true))//沒找到再找GLOBAL
				return false;//若還是沒找到就RETURN
	}
	else
	{
		if(!this->findObject(OBJECT::BASKETBALL::B_BASKET,100,true))//上次根本就沒找到，直接找GLOBAL
			return false;//若還是沒找到就RETURN
	}

	//有可能有找到，還要檢查輔助物
	std::vector<ObjectRegion>::iterator basket = info->objectRegion[OBJECT::BASKETBALL::B_BASKET].begin();
	for (;basket!=info->objectRegion[OBJECT::BASKETBALL::B_BASKET].end() ;basket++)
	{
		RegionOfInterest ROI;
		ROI.setRegion(basket->leftBoundary,basket->rightBoundary,basket->topBoundary-30,basket->topBoundary);
		if (this->findObject(OBJECT::BASKETBALL::B_BOARD,100,ROI,false))
		{//找到了!!

			basket->setBottomCenter2MainPoint();
			basket->robotCoordinate.y = info->strategy.adult.basketball.basketHeight;
			basket->transformImage2RobotCoordinate();
			basket->calcDisAndAngle();
			//clear all the other possible hoops, except the main one
			ObjectRegion tempRegion = (*basket);
			info->objectRegion[OBJECT::BASKETBALL::B_BASKET].clear();
			info->objectRegion[OBJECT::BASKETBALL::B_BASKET].push_back(tempRegion);

			return true;
		}
	}

	//最後很可以還是找不到
	info->objectRegion[OBJECT::BASKETBALL::B_BASKET].clear();
	return false;
}
bool Vision::findBasketball_2()
{
	if( this->findObject(OBJECT::BASKETBALL::B_BALL_2,800) )
	{
		std::vector<ObjectRegion>::iterator ball = info->objectRegion[OBJECT::BASKETBALL::B_BALL_2].begin();
		//due to the height of ball is not in the ground.
		/*ball->robotCoordinate.y = info->strategy.adult.basketball.ballHeight;*/
		ball->transformImage2RobotCoordinate();
		ball->calcDisAndAngle();

		if ( ball->dis > 2000 || ball->dis < 0 )
		{
			info->objectRegion[OBJECT::BASKETBALL::B_BALL_2].clear();
			return false;
		}

		return true;
	}
	return false;
}

/* -------------- Stairs ------------------ */
bool Vision::findStairs()
{
	return true;
}

/* --------- Lift and Carry -------------- */
bool Vision::findLevelOne()
{
	if ( this->findObject( OBJECT::LC::LEVELONE ,1000 ) )
	{
		std::vector<ObjectRegion>::iterator LvOne = info->objectRegion[OBJECT::LC::LEVELONE].begin();
		LvOne->transformImage2RobotCoordinate();
		LvOne->calcDisAndAngle();

		/* 尋找底端兩個頂點 計算底線的角度 */
		//找下面
		int i,j;
		j = LvOne->bottomBoundary;
		for (i = LvOne->leftBoundary ; i < LvOne->rightBoundary ; i++ )
		{
			if(LvOne->markedImage.at<unsigned char>(j,i))
				break;
		}
		cv::Point2d ObjectBottom(i,j);

		// 找左邊界點
		i = LvOne->leftBoundary;
		for (j = LvOne->bottomBoundary ; j > LvOne->topBoundary ; j-- )
		{
			if(LvOne->markedImage.at<unsigned char>(j,i))
				break;
		}
		cv::Point2d ObjectLeft(i,j);

		// 找右邊界點
		i = LvOne->rightBoundary;
		for (j = LvOne->bottomBoundary ; j > LvOne->topBoundary ; j-- )
		{
			if(LvOne->markedImage.at<unsigned char>(j,i))
				break;
		}
		cv::Point2d ObjectRight(i,j);
		
// 		double L_mid_X = 0;
// 		double L_mid_Y = 0;
// 		double R_mid_X = 0;
// 		double R_mid_Y = 0;
// 		L_mid_X = (ObjectBottom.x + ObjectLeft.x )/2;
// 		L_mid_Y = (ObjectBottom.y + ObjectLeft.y )/2;
// 		R_mid_X = (ObjectBottom.x + ObjectRight.x )/2;
// 		R_mid_Y = (ObjectBottom.y + ObjectRight.y )/2;

// 		cvPoint( L_mid_X,L_mid_Y );
// 		cvPoint( R_mid_X,R_mid_Y );
		cvLine(info->vision.camFrame,ObjectLeft,ObjectBottom,CV_RGB(255,255,0),10);
		cvLine(info->vision.camFrame,ObjectRight,ObjectBottom,CV_RGB(255,255,0),10);
		
		info->strategy.adult.liftcarry.LvOneLeftAng = -atan(( ObjectBottom.y - ObjectLeft.y )/( ObjectBottom.x - ObjectLeft.x ))*180/CV_PI;
		info->strategy.adult.liftcarry.LvOneRightAng = -atan(( ObjectBottom.y - ObjectRight.y )/( ObjectBottom.x - ObjectRight.x ))*180/CV_PI;
		info->strategy.adult.liftcarry.LvOneLeftDis = sqrt( ( ObjectBottom.y - ObjectLeft.y )*( ObjectBottom.y - ObjectLeft.y ) + ( ObjectBottom.x - ObjectLeft.x )*( ObjectBottom.x - ObjectLeft.x ) );
		info->strategy.adult.liftcarry.LvOneRightDis = sqrt( ( ObjectBottom.y - ObjectRight.y )*( ObjectBottom.y - ObjectRight.y ) + ( ObjectBottom.x - ObjectRight.x )*( ObjectBottom.x - ObjectRight.x ) );
		
		printf( "\nOne left = %0.f     Dis = %0.f\n",info->strategy.adult.liftcarry.LvOneLeftAng,info->strategy.adult.liftcarry.LvOneLeftDis );
		printf( "\nOne right = %0.f    Dis = %0.f\n",info->strategy.adult.liftcarry.LvOneRightAng,info->strategy.adult.liftcarry.LvOneRightDis );

		//決定哪一側才是主要的!
		if ( info->strategy.adult.liftcarry.LvOneLeftDis > info->strategy.adult.liftcarry.LvOneRightDis )
		{
			//左邊比較長 以左邊為主
// 			info->strategy.adult.liftcarry.LvOneLineMidPoint.x = L_mid_X;
// 			info->strategy.adult.liftcarry.LvOneLineMidPoint.y = L_mid_Y;
			info->strategy.adult.liftcarry.LvOneLength = info->strategy.adult.liftcarry.LvOneLeftDis;
			info->strategy.adult.liftcarry.LvOneAng = info->strategy.adult.liftcarry.LvOneLeftAng;
			
		}
		else if ( info->strategy.adult.liftcarry.LvOneLeftDis < info->strategy.adult.liftcarry.LvOneRightDis )
		{
			//右邊比較長 以右邊為主
// 			info->strategy.adult.liftcarry.LvOneLineMidPoint.x = R_mid_X;
// 			info->strategy.adult.liftcarry.LvOneLineMidPoint.y = R_mid_Y;
			info->strategy.adult.liftcarry.LvOneLength = info->strategy.adult.liftcarry.LvOneRightDis;
			info->strategy.adult.liftcarry.LvOneAng = info->strategy.adult.liftcarry.LvOneRightAng;
		}
		else
		{
			//長度一樣 以角度為主
			if ( info->strategy.adult.liftcarry.LvOneLeftAng > info->strategy.adult.liftcarry.LvOneRightAng )
			{
// 				info->strategy.adult.liftcarry.LvOneLineMidPoint.x = L_mid_X;
// 				info->strategy.adult.liftcarry.LvOneLineMidPoint.y = L_mid_Y;
				info->strategy.adult.liftcarry.LvOneLength = info->strategy.adult.liftcarry.LvOneLeftDis;
				info->strategy.adult.liftcarry.LvOneAng = info->strategy.adult.liftcarry.LvOneLeftAng;
			}
			else
			{
// 				info->strategy.adult.liftcarry.LvOneLineMidPoint.x = R_mid_X;
// 				info->strategy.adult.liftcarry.LvOneLineMidPoint.y = R_mid_Y;
				info->strategy.adult.liftcarry.LvOneLength = info->strategy.adult.liftcarry.LvOneRightDis;
				info->strategy.adult.liftcarry.LvOneAng = info->strategy.adult.liftcarry.LvOneRightAng;
			}
		}
		

		//計算線的中心點

// 		CvFont Font1 = cvFont(1,1); //(scal,thinkness)
// 		const char* leftAng = System::Convert::ToChar(info->strategy.adult.liftcarry.LvOneLeftAng);
// 		cvPutText(info->vision.camFrame,"123",ObjectBottom,&Font1,CV_RGB(0,255,0));
		return true;
	}
	else
	{
		info->objectRegion[OBJECT::LC::LEVELONE].clear();
		return false;
	}

}
bool Vision::findLevelTwo()
{
	return true;
}
bool Vision::findLevelThree()
{
	return true;
}

/*-------------通用函式--------------------*/
bool Vision::findObject( unsigned char objIndex )	// 1 parameter
{
	return findObject( objIndex ,1000 );
}
bool Vision::findObject( unsigned char objIndex ,int minPixelNumbers )	// 2 parameters
{
	return findObject( objIndex ,minPixelNumbers ,false );
}
bool Vision::findObject( unsigned char objIndex ,int minPixelNumbers ,bool isMulti )	//3 parameters
{
	if ( info->objectRegion[objIndex].empty() || isMulti )	//若之前沒找到或要找好幾個，就要搜尋整張影像
	{	//search global
		RegionOfInterest ROI;
		ROI.setRegion(0 ,this->camWidth-1 ,0 ,this->camHeight-1);

		if ( this->findObject(objIndex ,minPixelNumbers ,ROI ,isMulti) )
			return true;
	}
	else	
	{	//search local
		RegionOfInterest ROI;
		ROI.left = info->objectRegion[objIndex][0].leftBoundary;
		ROI.right = info->objectRegion[objIndex][0].rightBoundary;
		ROI.top = info->objectRegion[objIndex][0].topBoundary;
		ROI.bottom = info->objectRegion[objIndex][0].bottomBoundary;

		if ( this->findObject(objIndex ,minPixelNumbers ,ROI ,isMulti) )
			return true;

		//若local沒找到再找global
		ROI.setRegion(0 ,this->camWidth-1 ,0 ,this->camHeight);
		if ( this->findObject(objIndex ,minPixelNumbers ,ROI ,isMulti) )
			return true;
	}

	//not found
	info->objectRegion[objIndex].clear();
	return false;
}
bool Vision::findObject(unsigned char objIndex ,int minPixelNumbers ,RegionOfInterest ROI ,bool isMulti)	//4 parameters
{
	//initial
	ObjectRegion *tempRegion = new ObjectRegion;

	info->objectRegion[objIndex].clear();

	this->isPixelSearched.setTo(false);

	for (int j=ROI.bottom ;j>=ROI.top ;j--)
		//for (int j=ROI.top ;j<=ROI.bottom ;j++)
	{
		for (int i=ROI.left ;i<=ROI.right ;i++)
		{
			searchAround(i ,j ,(0x01<<objIndex) ,tempRegion);

			if (tempRegion->pixelNumbers > minPixelNumbers)
			{//found
				//assign object center
				tempRegion->setBottomCenter2MainPoint();

				//calculate object's robot coordinate
				tempRegion->transformImage2RobotCoordinate();

				//calculate object's distance and angle
				tempRegion->calcDisAndAngle();

				//push back
				info->objectRegion[objIndex].push_back(*tempRegion);

				delete tempRegion;
				if ( !isMulti )
					return true;	//find single
				else{
					tempRegion = new ObjectRegion;
					//tempRegion.initial();	//find multiple objects
				}
			}
			else
			{
				tempRegion->initial();
				/*return false;*/
			}
		}
	}

	delete tempRegion;
	if ( !info->objectRegion[objIndex].empty() )
		return true;
	else
		return false;
}

void Vision::searchAround(int i, int j ,unsigned short colorIndex ,ObjectRegion *objRegion)
{
	//out of boundary
	if ( i<0 ||i>this->camWidth-1 || j<0 || j>this->camHeight-1 )
		return;

	//avoid double check
	if ( this->isPixelSearched.at<unsigned char>(j,i) )
		return;

	//to be true after searching the pixel
	this->isPixelSearched.at<unsigned char>(j,i) = true;

	unsigned char h,s,v;
	h = this->camFrame_HSV->imageData[(j*this->camWidth+i)*3];
	s = this->camFrame_HSV->imageData[(j*this->camWidth+i)*3+1];
	v = this->camFrame_HSV->imageData[(j*this->camWidth+i)*3+2];

	if ( hsvObjLUT.color1[h] &hsvObjLUT.color2[s] &hsvObjLUT.color3[v] &colorIndex )
	{
		//mark image
		objRegion->markedImage.at<unsigned char>(j,i) = 255;	

		//update region
		if (objRegion->pixelNumbers == 0)
		{
			//initial boundary
			objRegion->leftBoundary = i;
			objRegion->rightBoundary = i;
			objRegion->topBoundary = j;
			objRegion->bottomBoundary = j;
		}
		else
		{
			if ( i > objRegion->rightBoundary )
				objRegion->rightBoundary = i;
			else if( i < objRegion->leftBoundary )
				objRegion->leftBoundary = i;

			if( j > objRegion->bottomBoundary )
				objRegion->bottomBoundary = j;
			else if ( j < objRegion->topBoundary )
				objRegion->topBoundary = j;
		}

		objRegion->pixelNumbers++;	//move to the next pixel, 符合條件的點+1

		searchAround(i+1 ,j ,colorIndex ,objRegion);	//right
		searchAround(i ,j-1 ,colorIndex ,objRegion);	//up
		searchAround(i-1 ,j ,colorIndex ,objRegion);	//left
		searchAround(i ,j+1 ,colorIndex ,objRegion);	//down

		searchAround(i+5 ,j ,colorIndex ,objRegion);	//right
		searchAround(i ,j-5 ,colorIndex ,objRegion);	//up
		searchAround(i-5 ,j ,colorIndex ,objRegion);	//left
		searchAround(i ,j+5 ,colorIndex ,objRegion);	//down

	}
	return;
}
void Vision::searchCorner(int i, int j ,unsigned short colorIndex )
{
	ObjectRegion objRegion;

	//out of boundary
	if ( i<0 ||i>this->camWidth-1 || j<0 || j>this->camHeight-1 )
		return;

	//avoid double check
	if ( this->isPixelSearched.at<unsigned char>(j,i) )
		return;

	//to be true after searching the pixel
	this->isPixelSearched.at<unsigned char>(j,i) = true;

	unsigned char p;
	p = this->dst_8U->imageData[(j*this->camWidth + i)];

	if ( p == colorIndex )
	{
		//mark image
		objRegion.markedImage.at<unsigned char>(j,i) = 255;	

		//update region
		if (objRegion.pixelNumbers == 0)
		{
			//initial boundary
			objRegion.leftBoundary = i;
			objRegion.rightBoundary = i;
			objRegion.topBoundary = j;
			objRegion.bottomBoundary = j;
		}
		else
		{
			if ( i > objRegion.rightBoundary )
				objRegion.rightBoundary = i;
			else if( i < objRegion.leftBoundary )
				objRegion.leftBoundary = i;

			if( j > objRegion.bottomBoundary )
				objRegion.bottomBoundary = j;
			else if ( j < objRegion.topBoundary )
				objRegion.topBoundary = j;
		}

		objRegion.pixelNumbers++;	//move to the next pixel, 符合條件的點+1

		searchCorner(i+1 ,j ,colorIndex );	//right
		searchCorner(i ,j-1 ,colorIndex );	//up
		searchCorner(i-1 ,j ,colorIndex );	//left
		searchCorner(i ,j+1 ,colorIndex );	//down

		searchCorner(i+5 ,j ,colorIndex );	//right
		searchCorner(i ,j-5 ,colorIndex );	//up
		searchCorner(i-5 ,j ,colorIndex );	//left
		searchCorner(i ,j+5 ,colorIndex );	//down

		

	}
		this->xmain = (objRegion.leftBoundary + objRegion.rightBoundary) / 2;
		this->ymain = (objRegion.topBoundary + objRegion.bottomBoundary) / 2;
	return;
}